package oneD.fem.element.linear;

import oneD.fem.Node;
import oneD.fem.element.Element;
import inf.jlinalg.Array2DMatrix;
import inf.jlinalg.IMatrix;
import inf.jlinalg.Vector3D;

public class Truss extends Element {
	protected double rho_;
	protected double sectionArea_;
	protected double eModulus_;

	public Truss(double rho, double e, double a, Node n1, Node n2) {
		this.rho_ = rho;
		this.eModulus_ = e;
		this.sectionArea_ = a;
		node_ = new Node[2];
		node_[0] = n1;
		node_[1] = n2;
		dofNumbers_ = new int[6];
	}

	public double getEModulus() {
		return this.eModulus_;
	}

	public double getArea() {
		return this.sectionArea_;
	}

	public Node getNode1() {
		return this.node_[0];
	}

	public Node getNode2() {
		return this.node_[1];
	}

	public double getLength() {
		Vector3D l = this.node_[1].getPosition().subtract(
				this.node_[0].getPosition());
		return l.norm2();
	}

	public Vector3D getE1() {
		return this.node_[1].getPosition()
				.subtract(this.node_[0].getPosition()).normalize();
	}

	/*public Vector getXe() {
		Vector Xe = new Vector(6);
		Xe.set(0, 3, this.getNode1().getPosition().toArray());
		Xe.set(3, 3, this.getNode2().getPosition().toArray());
		return Xe;
	}

	public Vector getUe() {
		Vector Ue = new Vector(6);
		Ue.set(0, 3, this.getNode1().getDisplacement().toArray());
		Ue.set(3, 3, this.getNode2().getDisplacement().toArray());
		return Ue;
	}*/

	@Override
	public IMatrix computeStiffnessMatrix() {
		IMatrix ke = new Array2DMatrix(6, 6);
		Vector3D Xe1e2 = this.node_[1].getPosition().subtract(
				this.node_[0].getPosition());
		IMatrix prod = Xe1e2.dyadicProduct(Xe1e2);
		double C = this.eModulus_ * this.sectionArea_
				/ Math.pow(this.getLength(), 3);
		for (int p = 0; p < 2; p++) {
			for (int q = 0; q < 2; q++) {
				double sign = -1;
				if ((p + q) % 2 == 0)
					sign = 1;
				for (int i = 0; i < 3; i++) {
					for (int j = 0; j < 3; j++) {
						ke.set(3 * p + i, 3 * q + j, sign * C * prod.get(i, j));
					}
				}
			}
		}
		/*System.out.println("Element " + this.getDOFNumbers().toString()
				+ " matrix listing");
		System.out.println("Linear stiffness:");
		System.out.print(MatrixFormat.format(ke));*/
		return ke;
	}

	@Override
	public IMatrix computeMassMatrix() {
//		double[][] med = new double[][] { { 2, 0, 1, 0, 1, 0 },
//				{ 0, 2, 0, 1, 0, 1 }, { 1, 0, 2, 0, 1, 0 },
//				{ 0, 1, 0, 2, 0, 1 }, { 1, 0, 1, 0, 2, 0 },
//				{ 0, 1, 0, 1, 0, 2 } };
		double[][] med = new double[][] { { 2, 0, 1, 0, 0, 0 },
				{ 0, 2, 0, 1, 0, 0 }, { 1, 0, 2, 0, 1, 0 },
				{ 0, 1, 0, 2, 0, 1 }, { 0, 0, 1, 0, 2, 0 },
				{ 0, 0, 0, 1, 0, 2 } }; //need check the exactness here
		IMatrix me = new Array2DMatrix(6, 6);
		for (int i = 0; i < 6; i++)
			for (int j = 0; j < 6; j++)
				me.set(i, j, this.rho_ * this.sectionArea_ * this.getLength()
						/ 6 * med[i][j]);
		return me;
	}

	@Override
	public double computeForce() {
		// compute Ti
		Vector3D e1 = this.getE1();
		double ue1 = e1.scalarProduct(this.node_[0].getDisplacement());
		double ue2 = e1.scalarProduct(this.node_[1].getDisplacement());
		return this.eModulus_ * this.sectionArea_ / this.getLength()
				* (ue2 - ue1);
	}

	@Override
	public void print() {
		System.out.print(this.eModulus_ + "\t" + this.sectionArea_ + "\t"
				+ this.getLength());
	}
}
